/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.dashboard;


import com.team1160.breakaway.api.Constants;
import com.team1160.breakaway.camera.TargetOrientation;
import com.team1160.breakaway.model.RobotState;
import com.team1160.breakaway.model.TargettingState;
import com.team1160.breakaway.model.TargettingStateEntry;
import edu.wpi.first.wpilibj.AnalogModule;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Dashboard;
import edu.wpi.first.wpilibj.DigitalModule;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Solenoid;

/**
 *
 * @author CJ
 */
public class DashBoard {
    /*
     * Output
     * 2-Left
     * 3-Center
     * 4-Right
     * 8-Dribbler
     * 
     * Input
     * 2 Kick
     * 4 Dribbler
     *
     */

    protected RobotState state;
    
    protected DriverStation driverStation;
    protected Dashboard dashboardLow;
    protected Dashboard dashboardHigh;
    protected DriverStationLCD driverLCD;

    protected boolean lastLCDValue=false;
    protected long lastTime;


    //-----------------------------------------------------------------
    
    public DashBoard()
    {
        this.state = null;

        //get our output
        this.driverStation = DriverStation.getInstance();
        this.dashboardHigh = this.driverStation.getDashboardPackerHigh();
        this.dashboardLow  = this.driverStation.getDashboardPackerLow();
        this.driverLCD     = DriverStationLCD.getInstance();
    }

    //-----------------------------------------------------------------
    
    public void update(RobotState state)
    {
        this.state = state;
        
        updateLEDs();

        updateDashboardBasicIO();

        updateDashboardTracking();

        updateDashboardMessages();
    }

    //-----------------------------------------------------------------

    protected void updateDashboardMessages()
    {
        driverLCD.println(DriverStationLCD.Line.kUser2,
                  1, "Updating the dashboard "+System.currentTimeMillis());

        String teamName =
                DriverStation.getInstance().getAlliance() == DriverStation.Alliance.kBlue ?
                    "BLUE" : "RED";
        String locaName = DriverStation.getInstance().getLocation() + "";

        driverLCD.println(DriverStationLCD.Line.kUser3,
                  1, "TEAM: "+Constants.TEAM_NAME + "   ");

        driverLCD.println(DriverStationLCD.Line.kUser4,
                  1, "ALLIANCE: "+teamName+ "   ");

        driverLCD.println(DriverStationLCD.Line.kUser5,
                  1, "LOCATION: "+locaName+ "   ");


        driverLCD.updateLCD();
    }

    //-----------------------------------------------------------------
    
    protected void updateDashboardTracking()
    {
        //dashboardHigh = DriverStation.getInstance().getDashboardPackerHigh();

        //Targeting Cluster
        dashboardHigh.addCluster();
        {
            //Tracking Data
            dashboardHigh.addCluster();
            {
                double x = 0.0, angle = 0.0, angularRate = 0.0;
                dashboardHigh.addDouble((double) x);
                dashboardHigh.addDouble((double) angle);
                dashboardHigh.addDouble((double) angularRate);
                dashboardHigh.addDouble((double) x);
            }
            dashboardHigh.finalizeCluster();

            //Target Info
            dashboardHigh.addCluster();
            {
                //Targets (1-dim array)
                dashboardHigh.addArray();
                {
                    addTargets(dashboardHigh);
                }
                dashboardHigh.finalizeArray();

                //TODO: timestamp is an int, get it from somewhere else
                int timestamp = (int) (System.currentTimeMillis() / 1000);
                dashboardHigh.addInt((int) timestamp);

            }
            dashboardHigh.finalizeCluster();

        }
        dashboardHigh.finalizeCluster();

        dashboardHigh.commit();
    }

    //---------------------------------------------------------------------

    /**
     * Adds target entries 
     * @param dashboard
     */
    protected void addTargets(Dashboard dashboard)
    {

        TargettingState tState = this.state.getTargettingInfo();

        int numEntries = tState.getSize();
        TargettingStateEntry entry = null;

        for (int i = 0; i < numEntries; ++i)
        {
            entry = tState.getEntry(i);

            //Target clusters
            dashboard.addCluster();
            {
                dashboard.addDouble(entry.getTargetScore());

                //circle descriptor
                dashboard.addCluster();
                {
                    //circle position
                    dashboard.addCluster();
                    {
                        dashboard.addInt(entry.getX());
                        dashboard.addInt(entry.getY());
                    }
                    dashboard.finalizeCluster(); //end_position


                    dashboard.addDouble(entry.getAngle());
                    dashboard.addDouble(entry.getMajorRadius());
                    dashboard.addDouble(entry.getMinorRadius());
                    dashboard.addDouble(entry.getRawScore());

                }
                dashboard.finalizeCluster(); //end_circle_desc

            }
            dashboard.finalizeCluster();

        } //end_for_loop
        
    }

    //---------------------------------------------------------------------


    protected void updateDashboardBasicIO()
    {
        //Dashboard dashboardLow = DriverStation.getInstance().getDashboardPackerLow();

        //BASIC IO Cluster
        dashboardLow.addCluster();
        {
            //2 x ANALOG Cluster
            dashboardLow.addCluster();
            {     
                //Analog Module 1
                dashboardLow.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        dashboardLow.addFloat((float) AnalogModule.getInstance(1).getAverageVoltage(i));
                    }
                }
                dashboardLow.finalizeCluster();

                //Analog Module 2
                dashboardLow.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        dashboardLow.addFloat((float) AnalogModule.getInstance(2).getAverageVoltage(i));
                    }
                }
                dashboardLow.finalizeCluster();
            }
            dashboardLow.finalizeCluster(); //end_analog_clusters


            //2 x Digital Modules
            dashboardLow.addCluster();
            {
                //Digital Module 1
                dashboardLow.addCluster();
                {
                    dashboardLow.addCluster();
                    {
                        int module = 4;
                        dashboardLow.addByte(DigitalModule.getInstance(module).getRelayForward());
                        dashboardLow.addByte(DigitalModule.getInstance(module).getRelayForward());
                        dashboardLow.addShort(DigitalModule.getInstance(module).getAllDIO());
                        dashboardLow.addShort(DigitalModule.getInstance(module).getDIODirection());
                        dashboardLow.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                dashboardLow.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        dashboardLow.finalizeCluster();
                    }
                    dashboardLow.finalizeCluster();
                }
                dashboardLow.finalizeCluster();

                //Digital Module 2
                dashboardLow.addCluster();
                {
                    dashboardLow.addCluster();
                    {
                        int module = 6;
                        dashboardLow.addByte(DigitalModule.getInstance(module).getRelayForward());
                        dashboardLow.addByte(DigitalModule.getInstance(module).getRelayReverse());
                        dashboardLow.addShort(DigitalModule.getInstance(module).getAllDIO());
                        dashboardLow.addShort(DigitalModule.getInstance(module).getDIODirection());
                        dashboardLow.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                dashboardLow.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        dashboardLow.finalizeCluster();
                    }
                    dashboardLow.finalizeCluster();
                }
                dashboardLow.finalizeCluster();

            }
            dashboardLow.finalizeCluster();

            //Solenoids
            //dashboardLow.addByte(Solenoid.getAll());
        }
        dashboardLow.finalizeCluster();
        dashboardLow.commit();

    }


    //-----------------------------------------------------------------

    protected void updateLEDs()
    {

        if(this.state.isDribblerRunning()){
            this.driverStation.setDigitalOut(Constants.LED_DRIBBLER, true);
        } else {
            this.driverStation.setDigitalOut(Constants.LED_DRIBBLER, false);
        }

        int orientationId = this.state.getTargetOrientation();

        switch (orientationId)
        {

            case TargetOrientation.LEFT:
                this.driverStation.setDigitalOut(Constants.LED_CAM_LEFT, true);
                this.driverStation.setDigitalOut(Constants.LED_CAM_CENTER, false);
                this.driverStation.setDigitalOut(Constants.LED_CAM_RIGHT, false);
                break;
            case TargetOrientation.RIGHT:
                this.driverStation.setDigitalOut(Constants.LED_CAM_LEFT, false);
                this.driverStation.setDigitalOut(Constants.LED_CAM_CENTER, false);
                this.driverStation.setDigitalOut(Constants.LED_CAM_RIGHT, true);
                break;
            case TargetOrientation.CENTER:
                this.driverStation.setDigitalOut(Constants.LED_CAM_LEFT, false);
                this.driverStation.setDigitalOut(Constants.LED_CAM_CENTER, true);
                this.driverStation.setDigitalOut(Constants.LED_CAM_RIGHT, false);
                break;
            default:
                if((System.currentTimeMillis()-this.lastTime)>2000)
                {
                    this.driverStation.setDigitalOut(Constants.LED_CAM_LEFT,   !this.lastLCDValue);
                    this.driverStation.setDigitalOut(Constants.LED_CAM_CENTER, !this.lastLCDValue);
                    this.driverStation.setDigitalOut(Constants.LED_CAM_RIGHT,  !this.lastLCDValue);
                    this.lastLCDValue = !this.lastLCDValue;
                    this.lastTime = System.currentTimeMillis();
                }
        }
    }


        void updateDashboardReference() {
        Dashboard lowDashData = DriverStation.getInstance().getDashboardPackerLow();


        //HIGHEST_CLUSTER_START
        lowDashData.addCluster();
        {
            // Analog_Module_1
            //   Analog_Module_1_voltage_1
            //    ...
            //   Analog_Module_1_voltage_8
            // End_Analog_Module_CLuster
            // Analog_Module_2
            //   Analog_Module_2_voltage_1
            //    ...
            //   Analog_Module_2_voltage_8
            // End_Analog_Module_Cluster

            //START_ANALOG
            lowDashData.addCluster();
            {   
                //analog modules (1 and 2)
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(1).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();

                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(2).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
            }
            lowDashData.finalizeCluster();


            //---------------------

            //DIGITAL_CLUSTER
            // Cluster_Start
            //  DigitalModel4
            //     RelayForward
            //     RelayForward
            //     AllDIO
            //     DIODirection
            //        PWM1
            //         ...
            //        PWM10
            // Cluster_End
            // Cluster_Start
            //  DigitalModel6
            //     RelayForward
            //     RelayForward
            //     AllDIO
            //     DIODirection
            //        PWM1
            //         ...
            //        PWM10
            // Cluster_End
            //  SOLENOIDS

            //START_ALL_DIGITAL
            lowDashData.addCluster();
            {
                //digital modules
                
                //start digital module 4
                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 4;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();
                //end digital modell 4

                //start digital module 6
                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 6;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayReverse());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();
                //end digital module 6

            }
            lowDashData.finalizeCluster();
            //end digital modules

            //add byte all solenoids
          //  lowDashData.addByte(Solenoid.getAll());
        }
        lowDashData.finalizeCluster();
        //END_ALL_DIGITAL


        lowDashData.commit();

    }
}
